import random
import math

import Drap

# ========== 智能体类 ==========#
class Robot(object):
    def __init__(self):
        self.vo = 0     # 目标速度
        self.vo_k = 0   # 目标速度方向
        self.Ox = 0  # 目标的横坐标
        self.Oy = 0  # 目标的纵坐标
        self.x = random.randint(0, 10)
        self.y = random.randint(0, 10)
        self.v = 2
        self.a = 3
        self.p = 1
        self.C_frict = -0.03
        self.C_shill_frict = -0.05
        self.C_ob_frict = -0.05
        self.v_flock = 1.5
        self.v_k = math.pi/4
        # 智能体间排斥
        self.p_prep = 1.5
        self.r0_rep = 2
        # 粘性速度
        self.v_frict = 5
        self.r0_frict = 100
        # 墙壁
        self.p_shill_prep = 3
        self.r0_shill_v = 30
        self.r0_shill_rep = 3
        self.r0_shill_frict = 30
        # 障碍物
        self.p_ob_prep = 1.5
        self.r0_ob_v = 5
        self.r0_ob_rep = 2
        self.r0_ob_frict = 30
        # 基站边缘点
        self.p_edge_prep = 1
        self.r0_edge_frict = 1
        self.edge_ob_frict = -0.002
        self.r0_edge_rep = 30

        self.gd = 3

    def xo(self):
        return self.x - self.Ox

    def yo(self):
        return self.y - self.Oy

    def r(self):
        return math.sqrt(self.xo()**2 + self.yo()**2)

    # 计算机器人当前位置与X轴的夹角
    def th(self):
        return math.atan2(self.yo(), self.xo())

    # 速度矢量差
    def v_ij(self):
        dx = self.v * math.cos(self.v_k) - self.vo * math.cos(self.vo_k)
        dy = self.v * math.sin(self.v_k) - self.vo * math.sin(self.vo_k)
        return math.sqrt(dx**2, dy**2)

    # 粘性速度方向
    def v_frict_th(self):
        dx = self.v * math.cos(self.v_k) - self.vo * math.cos(self.vo_k)
        dy = self.v * math.sin(self.v_k) - self.vo * math.sin(self.vo_k)
        return math.atan2(dy, dx)

    # 智能体的排斥速度
    def v_rep(self):
        if self.r < self.r0_rep:
            return self.p_prep * (self.r0_rep - self.r())
        else:
            return 0

    # ===== 墙壁 ===== #
    # 墙壁排斥速度
    def v_shill_rep(self):
        if self.r() < self.r0_shill_rep:
            return self.p_shill_prep * (self.r0_shill_rep - self.r())
        else:
            return 0

    # 墙壁粘性速度
    def v_shill_frict(self):
        v_shill_max = max(self.v_frict, Drap.drap(self.r()-self.r0_shill_frict, self.a, self.p))
        if self.v_ij() > v_shill_max:
            return self.C_shill_frict * (self.v_ij() - v_shill_max)
        else:
            return 0

    # ===== 障碍物 ===== #
    # 障碍物排斥速度
    def v_ob_rep(self):
        if self.r() < self.r0_ob_rep:
            return self.p_ob_prep * (self.r0_ob_rep - self.r())
        else:
            return 0

    # 障碍物粘性速度
    def v_ob_frict(self):
        v_ob_max = max(self.v_frict, Drap.drap(self.r()-self.r0_ob_frict, self.a, self.p))
        if self.v_ij() > v_ob_max:
            return self.C_ob_frict * (self.v_ij() - v_ob_max)
        else:
            return 0

    # ===== 基站边缘 ===== #
    # 基站边缘排斥速度
    def v_edge_rep(self):
        if self.r() < self.r0_edge_rep:
            return self.p_edge_prep * (self.r0_edge_rep - self.r())
        else:
            return 0

    # 障碍物粘性速度
    def v_ob_frict(self):
        v_edge_max = max(5, Drap.drap(self.r() - self.r0_edge_frict, self.a, self.p))
        if self.v_ij() > v_edge_max:
            return self.C_edge_frict * (self.v_ij() - v_edge_max)
        else:
            return 0

